#include "ht32f5xxxx_01.h"
#include "ht32f5xxxx_usart.h"
#include "ht32f5xxxx_ckcu.h"
#include "ht32f5xxxx_gpio.h"
#include "tim.h"
#include "bluetooth.h"
#include "spi.h"
#include "pwm.h"
#include "bldc_foc.h"
#include "ht32f5xxxx_pdma.h"
#include "AS5047P.h"
#include <math.h>

/*****************BLDC_Angle*****************/
#define pi 3.1415926
#define RAD2VALUE(x)  (x * 32768 / (2 * pi) + 32768)
/********************************************/

/**************Torque const*************/
#define MGL_Value      1234
/***************************************/

/**********************data format convert*********************/
#define TORQUE_ANPERE(x) ((x - 2048)/2048 * 4)
#define POSITION_RAD(x)  ((x - 32768)/32768 * (2 * pi))
#define RAD_POSITION(x)  ((x / 2 * pi) * 32768 + 32768)
/**************************************************************/


/*****************Push Rod Motor Control Macro*****************/
#define STRCAT2_(a, b)       a##b
#define STRCAT2(a, b)        STRCAT2_(a, b)
#define _PRM_CONTROL_PORT GPIOC
#define __PRM_CONTROL_PORT STRCAT2(HT_, _PRM_CONTROL_PORT)
#define __PRM_TB6612_A1    GPIO_PIN_1
#define __PRM_TB6612_A2    GPIO_PIN_2

#define PRMotorAdd()    \
                        GPIO_SetOutBits(__PRM_CONTROL_PORT, __PRM_TB6612_A1 | __PRM_TB6612_A2); \
                        GPIO_ClearOutBits(__PRM_CONTROL_PORT, __PRM_TB6612_A1)
#define PRMotorDec()    \
                        GPIO_SetOutBits(__PRM_CONTROL_PORT, __PRM_TB6612_A1 | __PRM_TB6612_A2); \
                        GPIO_ClearOutBits(__PRM_CONTROL_PORT, __PRM_TB6612_A2)
#define PRMotorSto()    \
                        GPIO_SetOutBits(__PRM_CONTROL_PORT, __PRM_TB6612_A1 | __PRM_TB6612_A2)
/**************************************************************/
extern unsigned int systick;

enum ARM_WorkMode
{Workmode_Stop = 0,Workmode_Positive , Workmode_Passive, Workmode_History, Workmode_Memory}ARM_WorkMode;

typedef struct PositionModeAngle
{
    uint16_t elbow_highest;
    uint16_t elbow_lowest;
    uint16_t swing_highest;
    uint16_t swing_lowest;
    uint16_t lift_highest;
    uint16_t lift_lowest;
    uint32_t current;
}PositionModeAngle;

#define PositionModeAngleSet(eh,el,sh,sl,lh,ll) \
                                            .elbow_highest = RAD2VALUE(eh), .elbow_lowest = RAD2VALUE(el), .swing_highest = RAD2VALUE(sh), .swing_lowest = RAD2VALUE(sl), .lift_highest = AS5047P_RAD2VALUE(lh), .lift_lowest = AS5047P_RAD2VALUE(ll)

void PositiveModeHandler(void);
void CKCU_Config(void);
void GPIO_Config(void);
void PositiveModeHandler(void);
void PassiveModeHandler(void);
void MemoryModeHandler(void);
void HistoryModeHandler(void);
void updateMotorData(void);
void PRMoterAdjust(Push_motor *PRMoter);
void HardFault_Handler(void);
void PushPopMotor_Initialize(Push_motor *target);
